#include "zuino.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

/*
#if defined(ARDUINO) && ARDUINO >= 100
#define printByte(args)  write(args);
#else
#define printByte(args)  print(args,BYTE);
#endif
*/

//#define ENABLE_SERIAL

LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
 
/////////////////////////////////////
// PIN CONFIGURATION
/////////////////////////////////////
// Motors
const int enableLR_pin = 3; 
const int input1L_pin = 7;  
const int input2L_pin = 8;
const int input1R_pin = 2;
const int input2R_pin = 4;

// RGB led
const int red_pin=13;   //no pwm: ho messo questo perchè sul 13 è meglio non aver input (c'è il led)
const int green_pin=9;
const int blue_pin=10;
// Buzzer
const int buzzer_pin = 11;
//Lcd 
const int scl_pin = A5;
const int sda_pin = A4;
// Buttons
const int buttonR_pin = 12;
const int buttonB_pin = 6;
// Light sensors
const int lightL_pin = A0;
const int lightR_pin = A1;
// Eyes (sonar)
const int eyeEcho_pin = A2; //Andrebbe bene digitale -> refactoring 
const int eyeTrig_pin = A3; //Andrebbe bene digitale -> refactoring
/////////////////////////////////////

const byte FORWARD = 0;
const byte REVERSE = 1;
const byte LEFT = 2;
const byte RIGHT = 3;

/////////////////////////////////////
// State variables
/////////////////////////////////////
byte robotDirection = 0;
byte robotVelocity = 0;
/////////////////////////////////////

ZButton blue_BUTTON = ZButton(buttonB_pin);
ZButton red_BUTTON = ZButton(buttonR_pin);
ZManager ZM;

boolean firstLoop = true;


/////////////////////////////////////
// For SONAR SENSOR
/////////////////////////////////////
const int numOfReadings = 10;                   // number of readings to take/ items in the array
int readings[numOfReadings];                    // stores the distance readings in an array
int arrayIndex = 0;                             // arrayIndex of the current item in the array
int total = 0;                                  // stores the cumlative total
int averageDistance = 0;                        // stores the average value
unsigned long pulseTime = 0;                    // stores the pulse in Micro Seconds
unsigned long distance = 0;                     // variable for storing the distance (cm)
//Follow hand
int previousAverageDistance = 0;
/////////////////////////////////////



void setup()
{  
  lcd.init();
  lcd.noBacklight();
  
  pinMode (enableLR_pin, OUTPUT);
  pinMode (input1R_pin, OUTPUT);
  pinMode (input2R_pin, OUTPUT);  
  pinMode (input1L_pin, OUTPUT);
  pinMode (input2L_pin, OUTPUT);
  
  pinMode(green_pin,OUTPUT);
  pinMode(blue_pin,OUTPUT);
  pinMode(red_pin,OUTPUT);

  pinMode(buzzer_pin,OUTPUT);
  
  pinMode(lightL_pin,INPUT);
  pinMode(lightR_pin,INPUT);
  
  pinMode(eyeTrig_pin, OUTPUT);  
  pinMode(eyeEcho_pin, INPUT);
  
  rgb_off();
  noTone(buzzer_pin);
  stopMotors();
  
  eyes_init();
  
  ZM.add(&blue_BUTTON); 
  ZM.add(&red_BUTTON);  
}

void loop()
{
  ZM.refresh();
  
  if(firstLoop)
  {  
    #ifdef ENABLE_SERIAL    
      Serial.begin(9600);
      Serial.println("First loop started");
    #endif
    lcd_hello();
    
    starting_grid();
    
    //police();
    
    lcd.noBacklight();
  

    //testMoving();
    stopMotors();
    
    rgb_multi();
 
    #ifdef ENABLE_SERIAL    
      Serial.println("First loop ended");
    #endif
    firstLoop = false;
  }
  else //next loops
  {
    if(blue_BUTTON.pressed())
    {
       lcd.backlight();
       delay(500);
    }
    
    if(red_BUTTON.pressed())
    {
       lcd.noBacklight();
       delay(500);
    }
    
    //followLight();  
    //eyes_read();
    //followHand();
    
    //stopMotors();
    delay(50);
  }  
}

/////////////////////////////////////
// MOVING AROUND
/////////////////////////////////////
void stopMoving()
{
  digitalWrite(enableLR_pin,LOW);
  digitalWrite(input1L_pin, LOW);  
  digitalWrite(input2L_pin, LOW);  
  digitalWrite(input1R_pin, LOW);  
  digitalWrite(input2R_pin, LOW);  
  robotVelocity = 0;
}

void stopMotors()
{
  stopMoving();
}

void move(byte dir)
{
  move(dir, 255);
}

void move(byte dir, byte vel)
{
  move(dir, vel, -1);
}

void move(byte dir, byte vel, int time)
{
  if(vel>255)
    vel = 255;
  if(vel<150)
    vel = 150;  
  
  robotDirection = dir;
  robotVelocity = vel;  

  
  switch ( dir ) 
  {
    case FORWARD:
      digitalWrite(input1L_pin, HIGH);  
      digitalWrite(input2L_pin, LOW);  
      digitalWrite(input1R_pin, LOW);  
      digitalWrite(input2R_pin, HIGH);       
      break;
    case REVERSE:
      digitalWrite(input1L_pin, LOW);  
      digitalWrite(input2L_pin, HIGH);  
      digitalWrite(input1R_pin, HIGH);  
      digitalWrite(input2R_pin, LOW);       
      break;
    case LEFT:
      digitalWrite(input1L_pin, LOW);  
      digitalWrite(input2L_pin, HIGH);  
      digitalWrite(input1R_pin, LOW);  
      digitalWrite(input2R_pin, HIGH);       
      break;
    case RIGHT:
      digitalWrite(input1L_pin, HIGH);  
      digitalWrite(input2L_pin, LOW);  
      digitalWrite(input1R_pin, HIGH);  
      digitalWrite(input2R_pin, LOW);       
      break;
    default:
      break;
  }
  
  analogWrite(enableLR_pin,vel);
  
  
  if(time>0)
  {
    delay (time);
    digitalWrite(enableLR_pin, LOW);  
  }
}

void testMoving()
{
  //Forward
  move(FORWARD,255, 1500);
  delay(1000);
  
  //Rewind
  move(REVERSE,255, 1500);
  delay(1000);

  //Left
  move(LEFT,255, 1000);
  delay(1000);
  
  //Right
  move(RIGHT,255, 1000);
  delay(1000);
  
  //Forward increasing and decreasing    
  for (int j = 150; j < 255; j += 10) 
  {
     move(FORWARD,j, 100);
  }
  for (int j = 255; j >= 150; j -= 10) 
  {
     move(FORWARD,j, 100);
  }
  stopMotors();
}
/////////////////////////////////////


/////////////////////////////////////
// RGB LED and BUZZER
/////////////////////////////////////
void rgb_off()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,LOW);
}

void rgb_white()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,HIGH);
}

void rgb_red()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,LOW);
}

void rgb_green()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,LOW);
}

void rgb_blue()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin, LOW);
  digitalWrite(blue_pin,HIGH);
}

void rgb_purple()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,HIGH);
}

void rgb_bho()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,LOW);
}

void rgb_yellow()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,HIGH);
}

void rgb_multi()
{
  rgb_blue();
  delay(250);
  rgb_yellow();
  delay(250);
  rgb_purple();
  delay(250);
  rgb_bho();
  delay(250);
  rgb_red();
  delay(250);
  rgb_green();
  delay(250);
  rgb_white();
  delay(250);
  rgb_off();
  delay(250);
}

void police()
{
  noTone(buzzer_pin);
  delay(100);
  for(int i=1; i<3; i++)
  {
    rgb_red();
    tone(buzzer_pin, 400);
    delay(700);
    noTone(buzzer_pin);
    rgb_off();
    delay(20);
    rgb_blue();
    tone(buzzer_pin, 600);
    delay(700);
    noTone(buzzer_pin);
    rgb_off();
    delay(20);     
  }
  noTone(buzzer_pin); 
}

void starting_grid()
{    
    noTone(buzzer_pin); 
    rgb_red();
    tone(buzzer_pin, 400);
    delay(400);
    rgb_off();
    noTone(buzzer_pin);
    delay(100);
    noTone(buzzer_pin); 
    rgb_red();
    tone(buzzer_pin, 400);
    delay(400);
    rgb_off();
    noTone(buzzer_pin);
    delay(100);
     noTone(buzzer_pin); 
    rgb_red();
    tone(buzzer_pin, 400);
    delay(400);
    rgb_off();
    noTone(buzzer_pin);
    delay(100);   
    rgb_green();
    tone(buzzer_pin, 600);
    delay(500);
    noTone(buzzer_pin);    
    rgb_off();
}
/////////////////////////////////////

/////////////////////////////////////
// Light sensors
/////////////////////////////////////
int deltaLight()
{
    int eyeR_light = analogRead(lightR_pin);
    int eyeL_light = analogRead(lightL_pin);
    
    int delta = eyeR_light - eyeL_light;

/*  
    #ifdef ENABLE_SERIAL
      Serial.print(eyeR_light);
      Serial.print("-");
      Serial.println(eyeL_light);   
      Serial.print(" -> ");
      Serial.println(delta); 
    #endif
*/    
    return delta;
}

void followLight()
{
  int delta = deltaLight();
  byte lightDirection;
  if(delta>0)
  {
    lightDirection = RIGHT;
  }
  if(delta<0)
  {
    lightDirection = LEFT;
    delta=-delta;
  }
  
  //led
  if(delta>3)
    if(lightDirection==LEFT)
      rgb_red();
    else
      rgb_green();
  else
    rgb_off();

  if(delta>6)
    move(lightDirection, 255, 200);
  else
  if(delta>4)
    move(lightDirection, 255, 75);  
    
  delay(100);  
}
/////////////////////////////////////

/////////////////////////////////////
// LCD display 
/////////////////////////////////////

void lcd_hello()
{
    lcd.home();
    lcd.print("I am Zuino...");
    lcd.setCursor(0, 1);
    lcd.print("... by Zulio");    
    lcd.backlight();  
}

/////////////////////////////////////

/////////////////////////////////////
// EYES sonar 
/////////////////////////////////////
void eyes_init()
{
  
  digitalWrite(eyeTrig_pin, LOW);
  // create array loop to iterate over every item in the array
  for (int thisReading = 0; thisReading < numOfReadings; thisReading++) 
  {
    readings[thisReading] = 0;
  }
}


void eyes_read()
{
  digitalWrite(eyeTrig_pin, HIGH);                    // send 10 microsecond pulse
  delayMicroseconds(10);                  // wait 10 microseconds before turning off
  digitalWrite(eyeTrig_pin, LOW);                     // stop sending the pulse
  pulseTime = pulseIn(eyeEcho_pin, HIGH);             // Look for a return pulse, it should be high as the pulse goes low-high-low
  distance = pulseTime/58;                        // Distance = pulse time / 58 to convert to cm.
   total= total - readings[arrayIndex];           // subtract the last distance
  readings[arrayIndex] = distance;                // add distance reading to array
  total= total + readings[arrayIndex];            // add the reading to the total
  arrayIndex = arrayIndex + 1;                    // go to the next item in the array
  // At the end of the array (10 items) then start again
  if (arrayIndex >= numOfReadings)  {
      arrayIndex = 0;
    }
  
  averageDistance = total / numOfReadings;      // calculate the average distance

/*  
  if(averageDistance<30)
    tone(buzzer_pin, averageDistance*100);
  else
    noTone(buzzer_pin);
*/    
  #ifdef ENABLE_SERIAL
    Serial.println(averageDistance, DEC);         // print out the average distance to the debugger
  #endif
  delay(50);     
}

void followHand()
{
  
  if(averageDistance<10)
  {
    rgb_red();
    move(REVERSE, 255, 200);
  }
  else
  {
  
    //SE la distanza rilevata è tra 10 e 40 indietroeggia
    if(averageDistance>20 && averageDistance<30)
    {
      rgb_green();
      move(FORWARD, 255, 200);
    }
    else
    {
      rgb_off();
      delay(100);
    }
  }
 /* 
  int delta = previousAverageDistance - averageDistance;
  previousAverageDistance = averageDistance;
  byte handDirection;
  if(delta>0)
  {
    handDirection = FORWARD;
  }
  if(delta<0)
  {
    handDirection = REVERSE;
    delta=-delta;
  }
  
  //led
  if(delta>2)
    if(handDirection==REVERSE)
      rgb_red();
    else
      rgb_green();
  else
    rgb_off();

  if(delta>6)
    move(handDirection, 255, 200);
  else
  if(delta>4)
    move(handDirection, 255, 75); 
  delay(100);  
 */   
  
}

/////////////////////////////////////
